/**
  ******************************************************************************
  * File Name          : CONTROL_TASK.c
  * Description        : ��������㷨ʵ��.
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2019 Little-Doggy.
  * All rights reserved.</center></h2>
  *
  ******************************************************************************
  */
	
#include "pid.h"
#include "main.h"
#include "control_task.h"	
#include "sensor_task.h"	
#include "trait.h"	
#include "gpio.h"	

#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

Body_t body;
extern void Ctrl_Task(void const * argument);

void BalanceTest(void)
{
	double x, y, z;
	x = -body.vertical_vec[1]*7;
	y = body.vertical_vec[2]*7;
	z = -body.vertical_vec[0]*7;
	Set_Feet_Pos(RF.index, x, y, z);
	Set_Feet_Pos(LF.index, x, y, z);
	Set_Feet_Pos(RB.index, x, y, z);
	Set_Feet_Pos(LB.index, x, y, z);
}

void TouchTest(void)
{
	
	switch (Feet_Get_TouchState(LF_LEG))
	{
	case OVER_TOUCH:
		/* code */
		LED_ON;
		LF.zero_y -= 0.12;
		break;
	case MOVE_TOUCH:
		/* code */
		LED_OFF;
		break;
	
	case STATIC_TOUCH:
		/* code */
		LED_OFF;
		break;
	
	case NONE_TOUCH:
		/* code */
		LED_ON;
		LF.zero_y += 0.12;
		break;
	
	default:
		break;
	}
	
	Set_Feet_Pos(LF_LEG, 0, LF.zero_y, 0);
	

}
void Ctrl_Task(void const * argument)
{
  /* init code for USB_DEVICE */
	extern void MX_USB_DEVICE_Init();
	MX_USB_DEVICE_Init();
	osDelay(3000);
	Leg_Init();
  /* USER CODE BEGIN Ctrl_Task */
  /* Infinite loop */
	for(;;)
	{
		BalanceTest();
		// TouchTest();
		// Main_Control_Task();
		osDelay(15);
	}
  /* USER CODE END Ctrl_Task */
}
double rotate_spd = 0, forward_spd = 0 ;
double trot_period = 0.45;
void Main_Control_Task()
{
	double t=0.0;
	
//	if(RC_CtrlData.rc.RightSwitch == RC_SWITCH_TOP)
//	{
//		forward_spd = 1.0*(RC_CtrlData.rc.RightY - 0x400)/(0x694-0x400);
//		rotate_spd  = -1.0*(RC_CtrlData.rc.RightX - 0x400)/(0x694-0x400);
//	}
//	else if(RC_CtrlData.rc.RightSwitch == RC_SWITCH_MIDDLE)
//	{
//		rotate_spd  = -1.0*(usb_face_x)/(300);
//		forward_spd = 0;
//	}
	
	 if (fabs(forward_spd) > 0.1 || fabs(rotate_spd) > 0.1)
	 {
		LF.Ay = RB.Ay = LB.Ay = RF.Ay = rotate_spd;
		RB.Ax = RF.Ax = 1*(forward_spd+0);
		LF.Ax = LB.Ax = 1*(forward_spd-0);
//		LF.zero_x = LF.Ax > 0? -1.2: 1.2;
//		RB.zero_x = RB.Ax > 0? -1.2: 1.2;
//		LB.zero_x = LB.Ax > 0? -1.2: 1.2;
//		RF.zero_x = RF.Ax > 0? -1.2: 1.2;
		LF.zero_x = 1.2;
		RB.zero_x = -1.2;
		LB.zero_x = -1.2;
		RF.zero_x = 1.2;
		
	 } else {
		
		LF.Ay = RB.Ay = LB.Ay = RF.Ay = 0;
		RB.Ax = RF.Ax = 0;
		LF.Ax = LB.Ax = 0;
		LF.zero_x = 0;
		RB.zero_x = 0;
		LB.zero_x = 0;
		RF.zero_x = 0;
		LF.zero_x = 2;
		RB.zero_x = -2;
		LB.zero_x = -2;
		RF.zero_x = 2;
		
	 }
	
	
	t = (HAL_GetTick()%(int)(trot_period*1000))/1000.0;
	
	
	
//	Set_Feet_Pos(RF.index, 0, 7, 0);
//	Set_Feet_Pos(LF.index, 0, 7, 0);
//	Set_Feet_Pos(RB.index, 0, 7, 0);
//	Set_Feet_Pos(LB.index, forward_spd, rotate_spd, 0);
	 
//	Trot_Leg(&RB, 		 2 * PI / trot_period * t);
//	Trot_Leg(&LB, PI + 2 * PI / trot_period * t);
//	Trot_Leg(&RF, PI + 2 * PI / trot_period * t);
//	Trot_Leg(&LF, 		 2 * PI / trot_period * t);
	
	
//	trot_leg_rectangle(&RB, 		 2 * PI / trot_period * t);
//	trot_leg_rectangle(&LB, PI + 2 * PI / trot_period * t);
//	trot_leg_rectangle(&RF, PI + 2 * PI / trot_period * t);
//	trot_leg_rectangle(&LF, 		 2 * PI / trot_period * t);
	
	Jump_Leg(&RF, 2 * PI / trot_period * t);
	Jump_Leg(&LF, 		 2 * PI / trot_period * t);
	Jump_Leg(&RB, 		 2 * PI / trot_period * t);
	Jump_Leg(&LB, 2 * PI / trot_period * t);

}

void BodyPos_Control_Task()
{
	
}

void BodyPos_Calc_Task()
{
	
}

